#ifndef KINEMATICS_TRICYCLE_H_
#define KINEMATICS_TRICYCLE_H_


#include "rclcpp/rclcpp.hpp"
#include <chassis_controller/pivots.h>
#include "chassis_controller/kinematics.h"
#include <chassis_controller/speed_limiter.h>
namespace chassis_controller
{
class TricycleKinematics : public Kinematics
{
public:
  explicit TricycleKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);

  void getCommand(double vx, double vy , double va);

  void getVelocity(double vx, double vy , double va);

  /// @brief 传入转舵和限速参数
  /// @param pivot_num  驱动单元数量
  /// @param steer_pid_param 转舵PID参数
  /// @param limiter_liner  速度限制
  /// @param limiter_steer  转舵速度限制
  void init(int pivot_num, pid_param steer_pid_param,SpeedLimiter limiter_liner, SpeedLimiter limiter_ang , SpeedLimiter limiter_steer);

  /// @brief 传入转舵的动态参数
  void updateParam(int pivot_num, pid_param steer_pid_param);

  /// @brief 根据底盘参数、当前的个驱动单元的速度，计算各驱动电机的速度或位置
  void update(std::vector<Spivot_value> &pivot_value, 
              std::vector<Spivot_param> &pivot_param , 
              std::vector<Spivot_setpoint> &pivot_setpoint ,const rclcpp::Duration &period,
              bool ready_move);

  /// @brief 目标速度或为位置赋值到hardware interface 
  void setCommand(std::vector<Spivot_value> &pivot_value, 
                  std::vector<Spivot_param> &pivot_param, 
                  std::vector<Spivot_setpoint> &pivot_setpoint,
                  const rclcpp::Duration &period);

  bool getSteeringFlag() ;
  bool getSteerErrFlag() ;
  bool getBrakerFlag();
  uint64_t getSteerCmdId() ;
  void setSteerCmdId( uint64_t steer_cmd_id ) ;
private:
  bool invert_flag_ = false;
  bool target_invert_flag_ = false;
  double liner_setpoint_ = 0.0;
  double angle_setpoint_ = 0.0;
  double angle_setpoint_last_valid = 0.0;
  PID PID_steer_;

  SpeedLimiter limiter_steer_;
  double last0_cmd_steer_ = 0.0;
  double last1_cmd_steer_ = 0.0;
  
  SpeedLimiter limiter_liner_;
  double last0_cmd_liner_ = 0.0;
  double last1_cmd_liner_ = 0.0;

  double max_liner_,min_liner_;
  double max_angular_,min_angular_;

  Sxya setpoint_;
  Sxya current_vel_;

  bool movement_;
  bool drive_request_,drive_request_old_,drive_req_trigger_;
  bool standstill_;
  bool steering_ ;
  bool steer_follow_err_;
  bool braker_open_;

  int pivots_num_;

  double steer_tolerance;
  double steer_follow_tolerance;
  double start_time_;

  uint64_t steer_cmd_id_;
  uint64_t steer_cmd_id_valid_;  
  double MINIMUM_TRANSLATIONAL_VELOCITY;
  bool USE_ANGLE_TOLERANCE;
  bool USE_LIMITED_STEER_ANGLE;
  // steer wheel relative origin position
  double pose_x_, pose_y_;
  // Wheel radius
  double radius_;
  double max_steer_;
  // motor rotational velocity and steer angle
  double motor_rotational_velocity_, servo_steer_angle_;
  int motor_direction_;
  // wheel velocity resolution 
  double desired_wheel_mount_velocity_x_;
  double desired_wheel_mount_velocity_y_;

void checkSteerDirection(double target, double actual, double max, double min, bool &positive_allowed, bool &negtive_allowed)
{
    positive_allowed = true;
    negtive_allowed = true;
    if (actual > min && actual < max)
    {
        if (target > max)
            positive_allowed = false;
        if (target < min)
            negtive_allowed = false;
    }

    if (actual > max)    
        positive_allowed = false;
    if (actual < min )
        negtive_allowed = false;    
}

double normAngle2(double angle)
{
    double fRad = angle;
    // Scale the angle into [-PI, +)
    while(fRad < -PI)
        fRad += 2*PI;
    // Scale the angle into [-PI, PI)
    while (fRad >= PI)
        fRad -= 2*PI;
    return fRad;
}
};
}
#endif
